#pragma once
#include <iostream>
#include <vector>
#include <queue>
#include <algorithm>
#include "Point.hpp"

struct Node
{
    Point position;
    int gCost;    // 起点到当前点的实际代价
    int hCost;    // 当前点到终点的预估代价
    Node *parent; // 父节点，用于回溯路径

    Node(const Point &pos, int g, int h, Node *par) : position(pos), gCost(g), hCost(h), parent(par) {}

    int fCost() const
    {
        return gCost + hCost;
    }
};
class PathPlanner
{
private:
    std::vector<std::vector<int>> grid; // 地图网格，0表示可通行，1表示障碍物
    Point start;
    Point end;

public:
    PathPlanner(const std::vector<std::vector<int>> &_grid, const Point &_start, const Point &_end)
        : grid(_grid), start(_start), end(_end) {}

    std::vector<Point> findPath()
    {
        std::vector<Point> path;
        if (grid.empty() || grid.size() <= end.x || grid[0].size() <= end.y || grid[end.x][end.y] == 1)
        {
            std::cout << "终点不可达" << std::endl;
            return path;
        }

        std::priority_queue<Node *, std::vector<Node *>, decltype(&compareNodes)> openSet(&compareNodes);
        //用来记录节点是否被访问过
        std::vector<std::vector<bool>> closedSet(grid.size(), std::vector<bool>(grid[0].size(), false));

        Node *startNode = new Node(start, 0, calculateHCost(start), nullptr);
        openSet.push(startNode);

        while (!openSet.empty())
        {
            Node *currentNode = openSet.top();
            openSet.pop();

            if (currentNode->position.x == end.x && currentNode->position.y == end.y)
            {
                path = generatePath(currentNode);
                break;
            }

            closedSet[currentNode->position.x][currentNode->position.y] = true;

            std::vector<Point> neighbors = getNeighbors(currentNode->position);
            for (const Point &neighbor : neighbors)
            {
                if (closedSet[neighbor.x][neighbor.y] || grid[neighbor.x][neighbor.y] == 1)
                    continue;

                int tentativeGCost = currentNode->gCost + 1; // 假设每次移动代价为1
                Node *neighborNode = new Node(neighbor, tentativeGCost, calculateHCost(neighbor), currentNode);

                bool inOpenSet = false;
                while(!openSet.empty())
                {
                    Node* node = openSet.top();
                    openSet.pop();
                    if (node->position.x == neighborNode->position.x && node->position.y == neighborNode->position.y)
                    {
                        inOpenSet = true;
                        if (tentativeGCost < node->gCost)
                        {
                            node->gCost = tentativeGCost;
                            node->parent = currentNode;
                        }
                        break;
                    }
                }

                if (!inOpenSet)
                {
                    openSet.push(neighborNode);
                }
            }
        }

        while (!openSet.empty())
        {
            delete openSet.top();
            openSet.pop();
        }
        return path;
    }

private:
    //建小堆
    static bool compareNodes(const Node *a, const Node *b)
    {
        return a->fCost() > b->fCost();
    }

    int calculateHCost(const Point &pos) const
    {
        // 使用曼哈顿距离作为启发函数（也可以使用其他启发函数）
        return abs(end.x - pos.x) + abs(end.y - pos.y);
    }

    std::vector<Point> getNeighbors(const Point &pos) const
    {
        std::vector<Point> neighbors;
        if (pos.x > 0)
            neighbors.emplace_back(pos.x - 1, pos.y); // 左
        if (pos.x < grid.size() - 1)
            neighbors.emplace_back(pos.x + 1, pos.y); // 右
        if (pos.y > 0)
            neighbors.emplace_back(pos.x, pos.y - 1); // 上
        if (pos.y < grid[0].size() - 1)
            neighbors.emplace_back(pos.x, pos.y + 1); // 下
        return neighbors;
    }

    std::vector<Point> generatePath(Node *endNode)
    {
        std::vector<Point> path;
        Node *currentNode = endNode;
        while (currentNode != nullptr)
        {
            path.push_back(currentNode->position);
            currentNode = currentNode->parent;
        }
        reverse(path.begin(), path.end()); // 由于是从终点向起点回溯，需要反转路径
        return path;
    }
};